#include <iostream>
#include "transform_utils/geometry_transform.h"

using namespace std;
using namespace Eigen;

int main(int argc, char const *argv[])
{
    double yaw = M_PI / 2;
    Quaterniond quad = Planner::ypr_to_quaternion(Vector3d(yaw, 0, 0));
    Vector3d p1(2, 3, 1);
    Isometry3d tran(quad);
    Vector3d p2 = tran * p1;
    cout << p2[0] << " " << p2[1] << " " << p2[2] << endl;

    return 0;
}
